# include "cartesian.h"
# include "veh_body.h"
# include "wheel.h"
# include "udpSettings.h"
# include "UdpCommunicate.h"

int main(int argc, char const *argv[])
{
    RC_udp::readXML();

    cpp_tools::udpVecReceiver<double> driveReceive(udpSettings::PORT::lower_sim::drive_len,udpSettings::PORT::lower_sim::drive,1);
    cpp_tools::udpVecReceiver<double> steeringReceive(udpSettings::PORT::lower_sim::steering_len,udpSettings::PORT::lower_sim::steering,1);

    cpp_tools::udpVecSender<double> stateSender(udpSettings::IP::lower,udpSettings::PORT::lower_sim::state,1);

    // param
    double T = 0.005; // s

    data::vehicle_param sys;
    double R_wheel = sys.m_tire.R_wheel;
    double inertial_spin = sys.m_tire.inertial_spin;
    double mass = sys.m_body.mass_sprung + sys.m_body.mass_unsprung_front + sys.m_body.mass_unsprung_rear
        + 4*sys.m_tire.mass_wheel;
    double inertial_z = sys.m_body.inertial_z;
    double l_f = sys.m_body.l_f;
    double l_r = sys.m_body.l_r;
    double l_y = sys.m_body.l_y;
    double h_cg = sys.m_body.h_cg;
    double g = sys.m_common.g;
    double l_x = l_f + l_r;

    // control
    double u_steer = 0;
    double u_drive = 0;

    // state
    double x = 0;
    double y = 0;
    double varphi = 0;

    double v = 0;
    double beta = 0;
    double omega = 0;

    double a_x = 0;
    double a_y = 0;

    Eigen::VectorXd F_z(4);

    // 初始化
    F_z << l_r/(l_f + l_r)*mass*g*0.5,l_r/(l_f + l_r)*mass*g*0.5,l_f/(l_f + l_r)*mass*g*0.5,l_f/(l_f + l_r)*g*mass*0.5;

    int count = 1;
    cpp_tools::clock base_clock;
    cpp_tools::clock loop_clock;
    while (1)
    {
        count++;

        auto t_loop = std::chrono::high_resolution_clock::now();

        // 接收控制量    
        if (steeringReceive.receive() == 0)
        {
            u_steer = steeringReceive.get()(0);
        }

        if (driveReceive.receive() == 0)
        {
            u_drive = driveReceive.get()(0);
        }

        double delta = 0;
        {
            const double p1 = 0.3529;
            const double p3 = 0.03441;
    
            delta = p1*u_steer + p3*std::pow(u_steer,3);

            // std::cout << "delta: " << delta << std::endl;
        }

        double F_x = 0;
        {
            const double mass = 5.2; // 标定时的整车质量

            if (u_drive >= 0)
            {
                const double p1 = -1.625*v + 30.1;
                const double p2 = -0.2403*v - 2.164;

                F_x = std::max(mass*(u_drive*p1 + p2),0.);
            }
            else
            {
                const double p1 = 5.023*v;
                const double p2 = 0.3972*v;

                F_x = std::min(mass*(u_drive*p1 + p2),0.);
            }

            // std::cout << "F_x: " << F_x << std::endl;
        }

        // ======================tire======================
        auto s_y = wheel_slip(v,beta,omega,delta,R_wheel,l_f,l_r,l_y);
        auto F_y_tire = MF98(s_y,F_z,sys.m_tire.MF_y);

        Eigen::VectorXd F_x_tire(4);
        F_x_tire << 0.25*F_x,0.25*F_x,0.25*F_x,0.25*F_x;
        for (size_t i = 0; i < 4; i++)
        {
            F_x_tire(i) = cpp_tools::saturation(0.25*F_x,-sys.m_tire.MF_x.D*F_z(i),sys.m_tire.MF_x.D*F_z(i));
            double d = std::sqrt(1 - std::pow(cpp_tools::saturation(F_y_tire(i)/(sys.m_tire.MF_y.D*F_z(i)),-1,1),2));
            F_x_tire(i) *= d;
            F_x_tire(i) -= cpp_tools::saturation(v,-1,1);
        }
        
        // ======================body dynamic======================
        Eigen::VectorXd veh_X(3);
        veh_X << v,beta,omega;
        Eigen::VectorXd veh_U(12);
        veh_U << delta,delta,0,0,
            F_x_tire,F_y_tire;
        auto [D_X,F_z_0,acc] = vehicle_body_dynamic(veh_X,veh_U,sys);
        
        v += D_X(0)*T;
        beta += D_X(1)*T;
        omega += D_X(2)*T;
        if (v < 0)
        {
            v = 0;
        }
        if (v < 1e-3)
        {
            beta = 0;
        }

        beta = cpp_tools::mod2pi(beta);

        a_x = acc(0);
        a_y = acc(1);

        double tau = 0.01;
        Eigen::VectorXd D_F_z = -1/tau*(F_z - F_z_0);
        F_z += D_F_z*T;

        // ======================cartesian======================
        auto [d_x,d_y,d_varphi] = cartesian_dynamic(v,beta,omega,varphi);

        x += d_x*T;
        y += d_y*T;
        varphi += d_varphi*T;

        varphi = cpp_tools::mod2pi(varphi);

        // 发送, 每10ms发送一次
        if (loop_clock.toc(0) >= 10)
        {
            std::cout 
            << "v: " << v << "\n"
            << "beta: " << beta << "\n"
            << "omega: " << omega << "\n"
            << "x: " << x << "\n"
            << "y: " << y << "\n"
            // << "F_x_tire: " << F_x_tire.transpose() << "\n"
            // << "F_y_tire: " << F_y_tire.transpose() << "\n"
            // << "count: " << count << "\n"
            << std::endl;
            
            loop_clock.tic();

            double uwb_dx_front = 0.216;
            double uwb_dx_rear = -0.258;
    
            Eigen::VectorXd position_uwb_front(2);
            Eigen::VectorXd position_uwb_rear(2);
    
            position_uwb_front << x + uwb_dx_front*std::cos(varphi), y + uwb_dx_front*std::sin(varphi);
            position_uwb_rear << x + uwb_dx_rear*std::cos(varphi), y + uwb_dx_rear*std::sin(varphi);
    
            Eigen::VectorXd state(udpSettings::PORT::lower_sim::state_len);
            state << position_uwb_front,position_uwb_rear,a_x,-a_y,-g,-0,0,-omega;
            stateSender.send(state + 0.000*Eigen::VectorXd::Random(udpSettings::PORT::lower_sim::state_len));

            // if (base_clock.toc(0) > 1e3)
            // {
            //     break;
            // }
        }

        // 等待
        std::this_thread::sleep_until(t_loop + std::chrono::microseconds(int(1e6*T)));
    }

    return 0;
}